import sys
sys.path.append('.\\lib')
# C:\Users\K\Desktop\ui\vision_tool\calib\program

from lib import xCoreSDK_python as rokae
import time
from util_3d import *


def waitRobot(robot):
    running = True
    while running:
        time.sleep(0.1)
        ec = {}
        st = robot.operationState(ec)
        if st == rokae.OperationState.idle.value or st == rokae.OperationState.unknown.value:
            running = False
class RobotArm:
    def __init__(self):
        ip = "192.168.2.160"
        ec = {}
        self.robot = rokae.xMateRobot(ip)
            # 连接机器人
        self.robot.connectToRobot(ec)
        self.robot.setOperateMode(rokae.OperateMode.automatic, ec)
        self.robot.setPowerState(True, ec)
        self.robot.moveReset(ec)
        self.robot.setDefaultZone(100, ec)
        self.robot.setDefaultSpeed(100, ec)

        # frame = self.robot.baseFrame(ec)
        # # self.robot.setBaseFrame(frame,ec)
        #
        # frame = rokae.Frame([0,0,0],[0,0,0])
        # self.robot.setBaseFrame(frame, ec)
        # frame = self.robot.baseFrame(ec)
        # print(frame)
        #
        # print(frame)
    def __init__(self, IP):
        ip = IP
        ec = {}
        self.robot = rokae.xMateRobot(ip)
        # 连接机器人
        self.robot.connectToRobot(ec)
        self.robot.setOperateMode(rokae.OperateMode.automatic, ec)
        self.robot.setPowerState(True, ec)
        self.robot.moveReset(ec)
        self.robot.setDefaultZone(100, ec)
        self.robot.setDefaultSpeed(100, ec)

    def change_auto_power_on(self):
        ec = {}
        self.robot.setOperateMode(rokae.OperateMode.automatic, ec)
        self.robot.setPowerState(True, ec)


    def movel_rokae(self, pose_rokae):
        ec = {}
        p5 = rokae.MoveLCommand(pose_rokae, 100, 300)
        self.robot.executeCommand([p5], ec)
        self.robot.moveStart(ec)
        self.wait_util()

    def movel_to_T(self, T):
        ec = {}
        pose = T_to_rokae_pose(T)
        p5 = rokae.MoveLCommand(pose, 100, 300)
        self.robot.executeCommand([p5], ec)
        self.robot.moveStart(ec)
        self.wait_util()

    def get_end_T(self):
        ec = {}
        posture = self.robot.flangePos(ec)
        T = rokae_pose_to_T(posture)
        return T

    def get_end_rokae_type(self):
        ec = {}
        posture = self.robot.cartPosture(rokae.CoordinateType.flangeInBase, ec)
        return posture.trans + posture.rpy

    def wait_util(self):
        running = True
        while running:
            time.sleep(0.1)
            ec = {}
            st = self.robot.operationState(ec)
            # print(st)
            if st == rokae.OperationState.idle or st == rokae.OperationState.unknown:
                running = False

    def is_arrival(self,pose):
        current_pose = self.get_end_rokae_type()
        print(pose)
        print(current_pose)
        for i in range(len(pose)):
            if not -0.01 < current_pose[i] - pose[i] < 0.01:
                return False
        return True

